
#include "LPC23xx.h"
#include "iflash.h"
#include "macros.h"
#include "debug.h"


typedef struct _SECTORGEOMETRY
{
   int size;
   int qty;
}SECTORGEOMETRY;

static int get_sector_number(unsigned int in_addr)
{
	static const SECTORGEOMETRY lpc2368_flash_sectors[] = // 2148
   {
      {4096,   8},   //-- 4K  * 8
      {32768, 14},   //-- 32K  *14
      {4096,   6},   //-- 4K  * 6 //5
      {0,      0}    //-- End
   };
 
	unsigned int end_addr;
   unsigned int start_addr;
   int n_sector;
   int i;
	//SECTORGEOMETRY* psg = NULL;
	const SECTORGEOMETRY* psg = NULL;

   start_addr = 0; //-- Flash start address
   n_sector = 0;
   //psg = (SECTORGEOMETRY*)&lpc2368_flash_sectors[0];
   psg = lpc2368_flash_sectors;
   while(psg->qty != 0)
   {
      for(i = 0; i < psg->qty; i++)
      {
         end_addr = start_addr + psg->size;
         if(in_addr >= start_addr && in_addr < end_addr)
            return n_sector;
         n_sector++;
         start_addr = end_addr;
      }
      psg++;
   }
   return (-1); //-- If here - err
}
 
static int flash_write(unsigned int flash_addr,
                unsigned int ram_addr,
                int prc_clk, //-- in KHz
                int len)     //-- 256,512, etc up to min sector size(4096)
{
   FWU_INTSAVE_DATA

   int n_sector;
   unsigned int rc = ERR_NO_ERR;

   n_sector = get_sector_number(flash_addr);
   if(n_sector == (-1)) //-- Err
      return ERR_WRONG_PARAM;

   fwu_disable_interrupt();

   //--- prepare to write
   rc = iap_command(IAP_CMD_PREPARE_SECTORS,n_sector,n_sector,0,0,0);
   if(rc == IAP_CMD_SUCCESS)
   {
      for(;;)
      {
         rc = iap_command(IAP_CMD_COPY_RAM_TO_FLASH,flash_addr,ram_addr,len,prc_clk,0);
         if(rc != IAP_BUSY)
            break;
      }
   }
   if(rc == IAP_CMD_SUCCESS)
      rc = ERR_NO_ERR;
   else
      rc = ERR_WSTATE;

   fwu_enable_interrupt();

   return rc;
}

//----------------------------------------------------------------------------
static int flash_erase_sectors(unsigned int start_addr,
                        int len,      //-- any
                        int prc_clk)  //-- in KHz
{
   FWU_INTSAVE_DATA

   int start_sector;
   int end_sector;
   unsigned int rc = ERR_NO_ERR;

   start_sector = get_sector_number(start_addr);
   end_sector   = get_sector_number(start_addr + len - 1);

   fwu_disable_interrupt();

/* 	DBG("\rstart_sector = %d", start_sector); */
/* 	DBG("\rend_sector = %d\r", end_sector); */


   if(start_sector != -1 && end_sector != -1)
   {
      rc = iap_command(IAP_CMD_PREPARE_SECTORS, start_sector, end_sector, 0, 0, 0);
      if(rc == IAP_CMD_SUCCESS)
      {
			//DBG("\r\tPREPARE_SECTORS\r");
         for(;;)
         {
            rc =  iap_command(IAP_CMD_ERASE_SECTORS, start_sector, end_sector, prc_clk, 0, 0);
            if(rc != IAP_BUSY)
               break;
         }
      }
   }

   fwu_enable_interrupt();

   return ERR_NO_ERR;
}

int fm_Erase ( void )
{
	return flash_erase_sectors(FM_ADDR_START, F_MEM_SIZE, (CCLK/1000) );
}

int fm_Write ( unsigned int flash_addr, void *data, unsigned int byte_count );
int fm_Write ( unsigned int flash_addr, void *data, unsigned int byte_count )
{
	unsigned int ram_addr = (unsigned int)data;
	
	flash_write(flash_addr, ram_addr, (CCLK/1000),byte_count);

	
	return 0;
} 

int fm_check_boot (void)
{
	unsigned int *p = (unsigned int*)FM_ADDR_START;

	DBG("\n%s; boot state: 0x%X\n",__FUNCTION__, *p); 

	if ( *p != GO_TO_MAIN_PROG )
	{
		fm_Erase();
		unsigned int b = GO_TO_MAIN_PROG;
		fm_Write(FM_ADDR_START,&b,256);
		for(b=0;b<1000000;b++);
	}

	DBG("\n%s; boot state: 0x%X\n",__FUNCTION__, *p); 

	return 0;
} 
